/*
 * Copyright 2018 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "cartographer/mapping/internal/range_data_collator.h"

#include <memory>

#include "absl/memory/memory.h"
#include "cartographer/mapping/internal/local_slam_result_data.h"
#include "glog/logging.h"

namespace cartographer
{
	namespace mapping
	{
		constexpr float RangeDataCollator::kDefaultIntensityValue;

		sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData(const std::string& sensor_id,
			sensor::TimedPointCloudData timed_point_cloud_data)
		{
			CHECK_NE(expected_sensor_ids_.count(sensor_id), 0);
			timed_point_cloud_data.intensities.resize(
				timed_point_cloud_data.ranges.size(), kDefaultIntensityValue);
			// TODO(gaschler): These two cases can probably be one.
			//https://zhuanlan.zhihu.com/p/415303501
			if (id_to_pending_data_.count(sensor_id) != 0)
			{
				current_start_ = current_end_;// 上一次裁剪合并的结束时间作为本次裁剪合并的开始时间
				// When we have two messages of the same sensor, move forward the older of the two (do not send out current).
				current_end_ = id_to_pending_data_.at(sensor_id).time;// 上一帧该传感器的点云数据中雷达点时间戳的最晚时间作为裁剪的结束时间
				auto result = CropAndMerge(); // 取出时间段内的点云数据返回
				id_to_pending_data_.emplace(sensor_id, std::move(timed_point_cloud_data));// 用新的数据替换原有的数据
				// 该分支的逻辑表示某一个雷达传感器的数据频率比别的传感器高(别的传感器数据还没来，就又来一帧)，就以该雷达的上一帧数据的时间
				// 戳作为结束时间进行集体裁剪然后返回，然后把当前到达的点云帧保存。
				return result;
			}
			id_to_pending_data_.emplace(sensor_id, std::move(timed_point_cloud_data));
			if (expected_sensor_ids_.size() != id_to_pending_data_.size())
			{
				return {};
			}
			// 所有的传感器数据都到了，进行集体裁剪
			current_start_ = current_end_;
			// We have messages from all sensors, move forward to oldest.
			common::Time oldest_timestamp = common::Time::max();
			for (const auto& pair : id_to_pending_data_)
			{
				oldest_timestamp = std::min(oldest_timestamp, pair.second.time);
			}
			current_end_ = oldest_timestamp;
			return CropAndMerge();
		}

		/*
		* 按照同一个时间段裁剪数据然后按照时间排序并合成一个点云数据。时间段的开始时间和结束时间不会重叠，且一直往后推移。
		* 所以总的来说这个类就是按照一个统一的时间段把所有传感器的数据进行裁剪，时间排序，合并然后返回。
		*/
		sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge()
		{
			sensor::TimedPointCloudOriginData result{ current_end_, {}, {} };
			bool warned_for_dropped_points = false;
			for (auto it = id_to_pending_data_.begin();
				it != id_to_pending_data_.end();)
			{
				sensor::TimedPointCloudData& data = it->second;
				const sensor::TimedPointCloud& ranges = it->second.ranges;
				const std::vector<float>& intensities = it->second.intensities;

				auto overlap_begin = ranges.begin();
				while (overlap_begin < ranges.end() &&
					data.time + common::FromSeconds((*overlap_begin).time) <
					current_start_)
				{
					++overlap_begin;
				}
				auto overlap_end = overlap_begin;
				while (overlap_end < ranges.end() &&
					data.time + common::FromSeconds((*overlap_end).time) <=
					current_end_)
				{
					++overlap_end;
				}
				if (ranges.begin() < overlap_begin && !warned_for_dropped_points)
				{
					LOG(WARNING) << "Dropped " << std::distance(ranges.begin(), overlap_begin)
						<< " earlier points.";
					warned_for_dropped_points = true;
				}

				// Copy overlapping range.
				if (overlap_begin < overlap_end)
				{
					std::size_t origin_index = result.origins.size();
					result.origins.push_back(data.origin);
					const float time_correction =
						static_cast<float>(common::ToSeconds(data.time - current_end_));
					auto intensities_overlap_it =
						intensities.begin() + (overlap_begin - ranges.begin());
					result.ranges.reserve(result.ranges.size() +
						std::distance(overlap_begin, overlap_end));
					for (auto overlap_it = overlap_begin; overlap_it != overlap_end;
						++overlap_it, ++intensities_overlap_it)
					{
						sensor::TimedPointCloudOriginData::RangeMeasurement point{
							*overlap_it, *intensities_overlap_it, origin_index };
						// current_end_ + point_time[3]_after == in_timestamp +
						// point_time[3]_before
						point.point_time.time += time_correction;
						result.ranges.push_back(point);
					}
				}

				// Drop buffered points until overlap_end.
				if (overlap_end == ranges.end())
				{
					it = id_to_pending_data_.erase(it);
				}
				else if (overlap_end == ranges.begin())
				{
					++it;
				}
				else
				{
					const auto intensities_overlap_end =
						intensities.begin() + (overlap_end - ranges.begin());
					data = sensor::TimedPointCloudData{
						data.time, data.origin,
						sensor::TimedPointCloud(overlap_end, ranges.end()),
						std::vector<float>(intensities_overlap_end, intensities.end()) };
					++it;
				}
			}

			std::sort(result.ranges.begin(), result.ranges.end(),
				[](const sensor::TimedPointCloudOriginData::RangeMeasurement& a,
					const sensor::TimedPointCloudOriginData::RangeMeasurement& b)
				{
					return a.point_time.time < b.point_time.time;
				});
			return result;
		}

	} // namespace mapping
} // namespace cartographer
